package com.zhdl.hardware.robot.borunte.repository.impl;

import com.google.common.eventbus.Subscribe;
import com.project.command.faq.event.behavior.BehaviorPerform;
import com.project.command.instruction.strategy.process.data.AfterStrategyParam;
import com.project.common.util.ByteHelper;
import com.zhdl.common.eventbus.center.EventBusCenter;
import com.zhdl.hardware.robot.borunte.repository.BorunteCmdHandleDao;
import com.zhdl.modules.tools.CommonCallback;
import com.zhdl.modules.tools.IntToHexUtil;
import com.zhdl.modules.tools.PublicTools;
import com.zhdl.network.modbus.dto.ModbusDto;
import com.zhdl.network.modbus.request.WriteSingleKeepRegisterReq;
import com.zhdl.network.modbustcp.ModbusTcpRespPkg;
import com.zhdl.network.strategy.common.ProtocolContext;
import com.zhdl.network.strategy.modbustcp.ModbusTcpStrategy;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import javax.annotation.Resource;
import java.io.IOException;
import java.util.Arrays;
import java.util.List;
import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;

@Slf4j
@Service
public class BorunteCmdHandleDaoImpl implements BorunteCmdHandleDao {

    /**
     * 机器人世界坐标-UVW轴姿态
     * 0 竖着 1 横着
     */
//    public static int[] UVW1 = new int[]{179700, 0, 90000};
//    public static int[] UVW0 = new int[]{179700, 0, 179700};
    public static int[] UVW = new int[]{180000, 0, 180000};
    /**
     * 机器人世界坐标-Z轴高度 机器人坐标类型
     */
    public static int originZ = 1670000;
    public static int originZ2 = 1100000;
    /**
     * 伯朗特机器人-网口
     */
    public String port;
    /**
     * 通信协议处理
     */
    public ProtocolContext protocolContext;
    /**
     * Callback 回调管理
     */
    public Map<String, CommonCallback<byte[]>> commonCallbackMap = new ConcurrentHashMap<>();

    @Resource
    public BehaviorPerform behaviorPerform;

    public void init(String port) {
        EventBusCenter.getEventBus().register(this);
        protocolContext = new ProtocolContext(new ModbusTcpStrategy(port, behaviorPerform));
        this.port = port;
    }

    @Subscribe
    public void dataReception(AfterStrategyParam event) {

        /* 判断当前串口 */
        if (port.equals(event.getPortKey())) {
            /* 获取电机指令返回的数据 */
            /* 此处接收串口或网口客户端回应代码,并处理 */
            List<ModbusDto> list = event.getNewData();
            ModbusDto modbusDto = list.get(0);
            String instruction = event.getInstruction();
            instruction = instruction.replaceAll(" ", "");
            instruction = instruction.toUpperCase();
            /* 遍历存放指令的集合，如何对应的map包含有指令，则取出来返回获取的数据 */
            for (String cmd : commonCallbackMap.keySet()) {
                String temp = cmd.toUpperCase();
                log.info("commonCallbackMap-----------------"+cmd);
                if (temp.contains(instruction)) {
                    commonCallbackMap.get(cmd).statusCallback(modbusDto.getUnitBuff());
                    log.info("当前指令:" + instruction + ",返回的数据为:" + Arrays.toString(modbusDto.getUnitBuff()));
                    commonCallbackMap.remove(cmd);
                }
            }
        }

    }

    /**
     * 通过字节数组转换成 16进制的String类型
     *
     * @param location 世界坐标
     * @return 16进制的世界坐标
     */
    public static String getHexByIntArray(int[] location) {
        StringBuilder stringBuffer = new StringBuilder();
        for (int j : location) {
            String s = IntToHexUtil.intToHex(j);
            stringBuffer.append(s);
        }
        return stringBuffer.toString();
    }

    /**
     * 伯朗特机器人-读取当前世界坐标
     * 0003000000060103091C000C
     */
    @Override
    public void borunteReadLocation(CommonCallback<byte[]> callback){
        String status = PublicTools.getString("cmd", "robotReadArmStatus");
        protocolContext.sendCmd(status);
        commonCallbackMap.put(status,callback);
        log.info("伯朗特机器人-读取世界坐标完毕");
    }

    /**
     * 伯朗特机器人-设置原点高度
     * 对应地址: 820(55A0)-825
     *
     * @param originLocation 当前位置XYZ 以及需要替换的Z
     */
    @Override
    public void borunteSetOriginZ(int[] originLocation) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setStartAddr("55B4");
        writeManyKeepRegisterReq.setId("01");
        originLocation[2] = originZ;
        writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(originLocation));
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteManyKeepRegister(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            protocolContext.sendCmd(s);
            log.info("Robot-设置原点高度" + s);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 伯朗特机器人-设置抓取点坐标
     * 设置 机器人抓取点 800(558C)-805
     *
     * @param pickLocation 抓取点 以及需要替换的Z
     */
    @Override
    public void borunteSetPickPoint(int[] pickLocation) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setStartAddr("558C");
        writeManyKeepRegisterReq.setId("01");
        writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(pickLocation) + getHexByIntArray(UVW));
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteManyKeepRegister(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            protocolContext.sendCmd(s);
            log.info("Robot-设置抓取点" + s);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 伯朗特机器人-设置机器人暂停
     * 设置 机器人暂停 open
     *
     * @param open true
     */
    @Override
    public void borunteSetPause(boolean open){
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setId("01");
        String address = open ? "4E21" : "4E23";
        writeManyKeepRegisterReq.setStartAddr(address);
        writeManyKeepRegisterReq.setWriteValue("0001");
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteSingleKeepRegister(writeManyKeepRegisterReq);
            protocolContext.sendCmd(ByteHelper.bytesArrayToHexString(bytes));
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 伯朗特机器人-设置抓取点坐标正上方
     * 设置 机器人抓取点 810(5596)-815
     *
     * @param pickLocation 抓取点 以及需要替换的Z
     */
    @Override
    public void borunteSetPickZ(int[] pickLocation) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setStartAddr("55A0");
        writeManyKeepRegisterReq.setId("01");
        pickLocation[2] = originZ;
        writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(pickLocation) + getHexByIntArray(UVW));
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteManyKeepRegister(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            protocolContext.sendCmd(s);
            /* 00 00 00 00 00 1f 01 10 55 a0 00 0c 18 ff d5 07 42 ff f7
               63 28 00 19 7b 70 00 02 bd f4 00 00 00 00 00 01 5f 90 */
            log.info("Robot-设置抓取点正上方" + s);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }
    /**
     * 伯朗特机器人-设置抓取点坐标正上方
     * 设置 机器人抓取点 810(5596)-815
     *
     * @param pickLocation 抓取点 以及需要替换的Z
     */
    @Override
    public void borunteSetPickZ2(int[] pickLocation) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setStartAddr("55A0");
        writeManyKeepRegisterReq.setId("01");
        pickLocation[2] = originZ2;
        writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(pickLocation) + getHexByIntArray(UVW));
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteManyKeepRegister(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            protocolContext.sendCmd(s);
            /* 00 00 00 00 00 1f 01 10 55 a0 00 0c 18 ff d5 07 42 ff f7
               63 28 00 19 7b 70 00 02 bd f4 00 00 00 00 00 01 5f 90 */
            log.info("Robot-设置抓取点正上方" + s);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 伯朗特机器人-读取开关量 M 系列信号
     * 读取 M10- 160(A0)至 M114线圈值
     *
     * @param address M信号地址
     */
    @Override
    public void borunteReadSwitch(int address, CommonCallback<byte[]> callback) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setId("01");
        writeManyKeepRegisterReq.setStartAddr(IntToHexUtil.shortToHex(address));
        writeManyKeepRegisterReq.setWriteValue("0001");
        try {
            byte[] bytes = ModbusTcpRespPkg.read01(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            protocolContext.sendCmd(s);
            commonCallbackMap.put(s+System.currentTimeMillis(),callback);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 伯朗特机器人-写入开关量 M 系列信号
     * 设置 M10- 160(A0)至 M114线圈值
     *
     * @param address M信号地址
     * @param open    true/false
     */
    @Override
    public void borunteWriteSwitch(int address, boolean open) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setId("01");
        writeManyKeepRegisterReq.setStartAddr(IntToHexUtil.shortToHex(address));
        writeManyKeepRegisterReq.setWriteValue(open ? "FF00" : "0000");
        try {
            byte[] bytes = ModbusTcpRespPkg.write05(writeManyKeepRegisterReq);
            protocolContext.sendCmd(ByteHelper.bytesArrayToHexString(bytes));
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }
}
